#pragma once
#include <zMat.hpp>
#include <Math/Vector.hpp>
#include <Utility/IOInterface.hpp>
#include <Utility/HasFlag.hpp>
#include <Utility/STLVector.hpp>
#include "../Feature/SIFTKeys.hpp"
#include <Xml/RapidXMLNode.hpp>
#include "ProjectionMat.hpp"

namespace zzz{
template<typename DataType>
class ProjectionMat;
enum{POINT_GOOD = 0,POINT_BADF, POINT_BADP};
enum{CAMERA_GOOD = 0, CAMERA_UNKNOWN};

static const zuint32 SD_IMAGE       = 0x00000001;
static const zuint32 SD_CAMERA      = 0x00000002;
static const zuint32 SD_POINT       = 0x00000004;

static const zuint32 SD_SIFTKEY     = 0x00000010;
static const zuint32 SD_SIFTMATCH   = 0x00000020;

static const zuint32 SD_SFMPAIR     = 0x00000100;

static const zuint32 SD_TRIPLANE    = 0x00001000;

static const zuint32 SD_ALL         = 0xffffffff;

template<typename DataType>
class PairData
{
public:
  ProjectionMat<DataType> cameras[2];
  STLVector<int> oriidx;
  STLVector<int> ptidx[2];
  STLVector<Vector<2,DataType> > pos2ds[2];
  STLVector<Vector<3,DataType> > pos3ds;
  STLVector<double> ferror;
  STLVector<double> perror;
  STLVector<zuint> status;
};

template<typename DataType=float>
class SceneData
{
public:
  const double Version;
  SceneData():Version(5.0){}

  ///////////////////////////reconstructed points
  struct Point2D
  {
    static const zuint32 BAD;
    typedef Vector<2,DataType> CoordXY;
    Point2D():img_id(BAD),pos2d(0),status(POINT_GOOD){}
    Point2D(int img_id_, const Vector<2,DataType> &pos2d_, int pt_idx_)
        : img_id(img_id_),pos2d(pos2d_),pt_idx(pt_idx_),status(POINT_GOOD){}
    zint32 img_id;
    zint32 pt_idx;
    Vector<2,DataType> pos2d;
    zuint32 status;
  };
  struct Point
  {
    Point():pos3d(0),ferror(0),perror(0),status(POINT_GOOD){}
    explicit Point(const Vector<3,DataType> &pos3d_):pos3d(pos3d_),ferror(0),perror(0),status(POINT_GOOD){}
    Vector<3,DataType> pos3d;
    STLVector<Point2D> pos2ds;
    DataType ferror;
    DataType perror;
    zuint32 status;
  };
  STLVector<Point> points_;

  ///////////////////////////image information
  struct ImageInfo
  {
    ImageInfo():size(0,0),focal(-1){}
    ImageInfo(const string &filename_, int sizex, int sizey):filename(filename_),size(sizex,sizey),focal(-1){}
    string filename;
    Vector<2,zint32> size;
    DataType focal;
  };
  STLVector<ImageInfo> imginfos_;

  ///////////////////////////reconstructed cameras
  struct CameraInfo
  {
    CameraInfo():status(CAMERA_UNKNOWN),k1(0),k2(0){}
    explicit CameraInfo(const ProjectionMat<DataType> &v):camera(v),status(CAMERA_GOOD),k1(0.0),k2(0.0){}
    ProjectionMat<DataType> camera;
    DataType k1,k2;
    zuint32 status;
  };
  STLVector<CameraInfo> cameras_;

  ///////////////////////////sift feature points
  STLVector<SIFTKeys> siftkeys_;

  ///////////////////////////siftkey pair matching
  struct SIFTMatch
  {
    typedef pair<zuint32, zuint32> PtIdx;
    typedef pair<Vector<2,DataType>, Vector<2,DataType> > Matches;
    static const zuint32 BAD;
    zuint32 id1,id2;
    STLVector<PtIdx> ptidx;
    //Coord_XY
    STLVector<Matches> matches;
  };
  STLVector<SIFTMatch> siftmatches_;

  ///////////////////////////SfM camera pair
  struct SfMPair
  {
    int id1,id2;
    zuint32 matches;
    zuint32 inliers;
    Matrix<3,3,DataType> rot;
    Vector<3,DataType> trans;
  };
  STLVector<SfMPair> sfmpairs_;

  ///////////////////////////Triangle planes
  struct TriPlane
  {
    Vector3i pts;
    DataType error;
  };
  STLVector<TriPlane> triplanes_;

  ////////////////////////////Generator
  STLVector<string> generators_;

  void Clear()
  {
    points_.clear();
    imginfos_.clear();
    cameras_.clear();
    siftkeys_.clear();
    siftmatches_.clear();
    sfmpairs_.clear();
    triplanes_.clear();
    generators_.clear();
  }

  void SaveToFileXml(RapidXMLNode &node, zuint flags=SD_ALL)
  {
    node.SetAttribute("Version",Version);

    if (CheckBit(flags,SD_IMAGE))
    {
      RapidXMLNode imgnode=node.AppendNode("ImageInfoSet");
      imgnode.SetAttribute("Number",imginfos_.size());
      for (zuint i=0; i<imginfos_.size(); i++)
      {
        RapidXMLNode inode=imgnode.AppendNode("ImageInfo");
        inode.SetAttribute("ID",i);
        inode.SetAttribute("FileName",imginfos_[i].filename);
        inode.SetAttribute("Size",imginfos_[i].size);
        inode.SetAttribute("Focal",imginfos_[i].focal);
      }
    }

    if (CheckBit(flags,SD_CAMERA))
    {
      RapidXMLNode cameranode=node.AppendNode("CameraSet");
      cameranode.SetAttribute("Number",cameras_.size());
      for (zuint i=0; i<cameras_.size(); i++)
      {
        RapidXMLNode cnode=cameranode.AppendNode("Camera");
        cnode.SetAttribute("ID",i);
        cnode.SetAttribute("K1",cameras_[i].k1);
        cnode.SetAttribute("K2",cameras_[i].k2);
        cnode.SetAttribute("Status",cameras_[i].status);
        cnode.AppendNode("K")<<cameras_[i].camera.K();
        cnode.AppendNode("R")<<cameras_[i].camera.R();
        cnode.AppendNode("T")<<cameras_[i].camera.T();
        cnode.AppendNode("P")<<cameras_[i].camera.P();
      }
    }

    if (CheckBit(flags,SD_POINT))
    {
      RapidXMLNode pointnode=node.AppendNode("PointSet");
      pointnode.SetAttribute("Number",points_.size());
      for (zuint i=0; i<points_.size(); i++)
      {
        RapidXMLNode pnode=pointnode.AppendNode("Point");
        pnode.SetAttribute("Pos3D",points_[i].pos3d);
        pnode.SetAttribute("F_Error",points_[i].ferror);
        pnode.SetAttribute("P_Error",points_[i].perror);
        pnode.SetAttribute("Status",points_[i].status);
        pnode.SetAttribute("Number",points_[i].pos2ds.size());
        for (zuint j=0; j<points_[i].pos2ds.size(); j++)
        {
          RapidXMLNode p2node=pnode.AppendNode("Pos2D");
          p2node.SetAttribute("ID",points_[i].pos2ds[j].img_id);
          p2node.SetAttribute("PtIdx",points_[i].pos2ds[j].pt_idx);
          Vector<2,DataType> pos = points_[i].pos2ds[j].pos2d + Vector<2,DataType>(imginfos_[points_[i].pos2ds[j].img_id].size/2);
          p2node.SetAttribute("Pos",pos);
          p2node.SetAttribute("Status",points_[i].pos2ds[j].status);
        }
      }
    }

    if (CheckBit(flags,SD_SIFTKEY))
    {
      RapidXMLNode siftnode=node.AppendNode("SIFTKeySet");
      siftnode.SetAttribute("Number",siftkeys_.size());
      for (zuint i=0; i<siftkeys_.size(); i++)
      {
        RapidXMLNode ssnode=siftnode.AppendNode("SIFTKeys");
        ssnode.SetAttribute("Number",siftkeys_[i].keys_.size());
        ssnode.SetAttribute("ID",i);
        for (zuint j=0; j<siftkeys_[i].keys_.size(); j++)
        {
          RapidXMLNode snode=ssnode.AppendNode("SIFTKey");
          snode.SetAttribute("X",siftkeys_[i].keys_[j].pos[0]);
          snode.SetAttribute("Y",siftkeys_[i].keys_[j].pos[1]);
          snode.SetAttribute("Scale",siftkeys_[i].keys_[j].scale);
          snode.SetAttribute("Dir",siftkeys_[i].keys_[j].dir);
          Vector<128,zushort> desc(siftkeys_[i].desc_[j]);
          snode<<desc;
        }
      }
    }

    if (CheckBit(flags,SD_SIFTMATCH))
    {
      RapidXMLNode smatchnode=node.AppendNode("SIFTMatchSet");
      smatchnode.SetAttribute("Number",siftmatches_.size());
      for (zuint i=0; i<siftmatches_.size(); i++)
      {
        RapidXMLNode ssnode=smatchnode.AppendNode("SIFTMatches");
        ssnode.SetAttribute("Number",siftmatches_[i].matches.size());
        ssnode.SetAttribute("ID1",siftmatches_[i].id1);
        ssnode.SetAttribute("ID2",siftmatches_[i].id2);
        for (zuint j=0; j<siftmatches_[i].matches.size(); j++)
        {
          RapidXMLNode mnode=ssnode.AppendNode("Match");
          mnode.SetAttribute("P1",siftmatches_[i].matches[j].first);
          mnode.SetAttribute("P2",siftmatches_[i].matches[j].second);
          mnode.SetAttribute("PtIdx1",siftmatches_[i].ptidx[j].first);
          mnode.SetAttribute("PtIdx2",siftmatches_[i].ptidx[j].second);
        }
      }
    }

    if (CheckBit(flags,SD_SFMPAIR))
    {
      RapidXMLNode cpnode=node.AppendNode("SfMPairSet");
      cpnode.SetAttribute("Number",sfmpairs_.size());
      for (zuint i=0; i<sfmpairs_.size(); i++)
      {
        RapidXMLNode cnode=cpnode.AppendNode("SfMPair");
        cnode.SetAttribute("ID1",sfmpairs_[i].id1);
        cnode.SetAttribute("ID2",sfmpairs_[i].id2);
        cnode.SetAttribute("Matches",sfmpairs_[i].matches);
        cnode.SetAttribute("Inliers",sfmpairs_[i].inliers);
        cnode.SetAttribute("Rotation",sfmpairs_[i].rot);
        cnode.SetAttribute("Translation",sfmpairs_[i].trans);
      }
    }

    if (CheckBit(flags,SD_TRIPLANE))
    {
      RapidXMLNode tripnode=node.AppendNode("TriPlaneSet");
      tripnode.SetAttribute("Number",triplanes_.size());
      for (zuint i=0; i<triplanes_.size(); i++)
      {
        RapidXMLNode tnode=tripnode.AppendNode("TriPlane");
        tnode.SetAttribute("Pts",triplanes_[i].pts);
        tnode.SetAttribute("Error",triplanes_[i].error);
      }
    }

    {
      RapidXMLNode gsnode=node.AppendNode("Generators");
      for (zuint i=0; i<generators_.size(); i++)
        gsnode.AppendNode("Generator")<<generators_[i];
    }
  }

  void LoadFromFileXml(RapidXMLNode &node,zuint flags=SD_ALL)
  {
    double version=99999;
    node.GetAttribute("Version",version);
    if (version>Version)
        {
            ZLOG(ZERROR)<<"This is not valid file or a file written by a higher version of me\n";
            return;
        }

    Clear();

    if (CheckBit(flags,SD_IMAGE) && node.HasNode("ImageInfoSet"))
    {
      RapidXMLNode imgnode=node.GetNode("ImageInfoSet");
      int imgn;
      imgnode.GetAttribute("Number",imgn);
      imginfos_.assign(imgn,ImageInfo());
      for (RapidXMLNode inode=imgnode.GetFirstNode("ImageInfo");inode.IsValid();inode.GotoNextSibling("ImageInfo"))
      {
        int id=FromString<int>(inode.GetAttribute("ID"));
        imginfos_[id].filename=inode.GetAttribute("FileName");
        imginfos_[id].size=FromString<Vector<2,DataType> >(inode.GetAttribute("Size"));
        inode.GetAttribute("Focal",imginfos_[id].focal);
      }
    }

    if (CheckBit(flags,SD_CAMERA) && node.HasNode("CameraSet"))
    {
      RapidXMLNode cameranode=node.GetNode("CameraSet");
      int camn;
      cameranode.GetAttribute("Number",camn);
      cameras_.assign(camn,CameraInfo());
      for (RapidXMLNode cnode=cameranode.GetFirstNode("Camera");cnode.IsValid();cnode.GotoNextSibling("Camera"))
      {
        int id=FromString<int>(cnode.GetAttribute("ID"));
        cnode.GetAttribute("K1",cameras_[id].k1);
        cnode.GetAttribute("K2",cameras_[id].k2);
        cnode.GetAttribute("Status",cameras_[id].status);
        cameras_[id].camera.SetK(FromString<Matrix<3, 3, DataType> >(cnode.GetNode("K").GetText()));
        cameras_[id].camera.SetRT(FromString<Matrix<3, 3, DataType> >(cnode.GetNode("R").GetText()),\
                   FromString<Vector<3, DataType> >(cnode.GetNode("T").GetText()));
        cameras_[id].camera.SetP(FromString<Matrix<3,4,DataType> >(cnode.GetNode("P").GetText()));
      }
    }

    if (CheckBit(flags,SD_POINT) && node.HasNode("PointSet"))
    {
      RapidXMLNode pointnode=node.GetNode("PointSet");
      int pointn;
      pointnode.GetAttribute("Number",pointn);
      points_.reserve(pointn);
      for (RapidXMLNode pnode=pointnode.GetFirstNode("Point");pnode.IsValid();pnode.GotoNextSibling("Point"))
      {
        points_.push_back(Point());
        Point &point=points_.back();
        pnode.GetAttribute("Pos3D",point.pos3d);
        pnode.GetAttribute("F_Error",point.ferror);
        pnode.GetAttribute("P_Error",point.perror);
        pnode.GetAttribute("Status",point.status);
        int pos2dn;
        pnode.GetAttribute("Number",pos2dn);
        point.pos2ds.reserve(pos2dn);
        for (RapidXMLNode p2node=pnode.GetFirstNode("Pos2D");p2node.IsValid();p2node.GotoNextSibling("Pos2D"))
        {
          Point2D pos2d;
          p2node.GetAttribute("ID",pos2d.img_id);
          p2node.GetAttribute("PtIdx",pos2d.pt_idx);
          pos2d.pos2d=FromString<Vector<2,DataType> >(p2node.GetAttribute("Pos")) - Vector<2,DataType>(imginfos_[pos2d.img_id].size/2);
          p2node.GetAttribute("Status",pos2d.status);
          point.pos2ds.push_back(pos2d);
        }
      }
    }

    if (CheckBit(flags,SD_SIFTKEY) && node.HasNode("SIFTKeySet"))
    {
      RapidXMLNode siftnode=node.GetNode("SIFTKeySet");
      int setn;
      siftnode.GetAttribute("Number",setn);
      siftkeys_.assign(setn,SIFTKeys());
      for (RapidXMLNode ssnode=siftnode.GetNode("SIFTKeys");ssnode.IsValid();ssnode.GotoNextSibling("SIFTKeys"))
      {
        int id;
        ssnode.GetAttribute("ID",id);
        SIFTKeys &siftkeys=siftkeys_[id];
        int keyn;
        ssnode.GetAttribute("Number",keyn);
        siftkeys.keys_.reserve(keyn);
        siftkeys.desc_.reserve(keyn);
        for (RapidXMLNode snode=ssnode.GetNode("SIFTKey");snode.IsValid();snode.GotoNextSibling("SIFTKey"))
        {
          SIFTKey key;
          snode.GetAttribute("X",key.pos[0]);
          snode.GetAttribute("Y",key.pos[1]);
          snode.GetAttribute("Scale",key.scale);
          snode.GetAttribute("Dir",key.dir);
          siftkeys.keys_.push_back(key);
          Vector<128,zushort> desc=FromString<Vector<128,zushort> >(snode.GetText());
          siftkeys.desc_.push_back(Vector<128,zuchar>(desc));
        }
      }
    }

    if (CheckBit(flags,SD_SIFTMATCH) && node.HasNode("SIFTMatchSet"))
    {
      RapidXMLNode smatchnode=node.GetNode("SIFTMatchSet");
      int setn;
      smatchnode.GetAttribute("Number",setn);
      siftmatches_.reserve(setn);
      for (RapidXMLNode ssnode=smatchnode.GetNode("SIFTMatches");ssnode.IsValid();ssnode.GotoNextSibling("SIFTMatches"))
      {
        siftmatches_.push_back(SIFTMatch());
        SIFTMatch &match=siftmatches_.back();
        ssnode.GetAttribute("ID1",match.id1);
        ssnode.GetAttribute("ID2",match.id2);
        int matchn;
        ssnode.GetAttribute("Number",matchn);
        match.matches.reserve(matchn);
        match.ptidx.reserve(matchn);
        for (RapidXMLNode mnode=ssnode.GetNode("Match");mnode.IsValid();mnode.GotoNextSibling("Match"))
        {
          pair<Vector<2,DataType>,Vector<2,DataType> > p;
          mnode.GetAttribute("P1",p.first);
          mnode.GetAttribute("P2",p.second);
          match.matches.push_back(p);
          pair<int,int> ptidx;
          mnode.GetAttribute("PtIdx1",ptidx.first);
          mnode.GetAttribute("PtIdx2",ptidx.second);
          match.ptidx.push_back(ptidx);
        }
      }
    }  

    if (CheckBit(flags,SD_SFMPAIR) && node.HasNode("SfMPairSet"))
    {
      RapidXMLNode cpnode=node.GetNode("SfMPairSet");
      int setn;
      cpnode.GetAttribute("Number",setn);
      sfmpairs_.reserve(setn);
      for (RapidXMLNode cnode=cpnode.GetNode("SfMPair");cnode.IsValid();cnode.GotoNextSibling("SfMPair"))
      {
        SfMPair p;
        cnode.GetAttribute("ID1",p.id1);
        cnode.GetAttribute("ID2",p.id2);
        cnode.GetAttribute("Matches",p.matches);
        cnode.GetAttribute("Inliers",p.inliers);
        cnode.GetAttribute("Rotation",p.rot);
        cnode.GetAttribute("Translation",p.trans);
        sfmpairs_.push_back(p);
      }
    }

    if (CheckBit(flags,SD_TRIPLANE) && node.HasNode("TriPlaneSet"))
    {
      RapidXMLNode tripnode=node.GetNode("TriPlaneSet");
      int setn;
      tripnode.GetAttribute("Number",setn);
      for (RapidXMLNode tnode=tripnode.GetNode("TriPlane");tnode.IsValid();tnode.GotoNextSibling("TriPlane"))
      {
        TriPlane tri;
        tnode.GetAttribute("Pts",tri.pts);
        tnode.GetAttribute("Error",tri.error);
        triplanes_.push_back(tri);
      }
    }

    if (node.HasNode("Generators"))
    {
      RapidXMLNode gsnode=node.GetNode("Generators");
      for (RapidXMLNode gnode=gsnode.GetNode("Generator");gnode.IsValid();gnode.GotoNextSibling("Generator"))
        generators_.push_back(string(gnode.GetText()));
    }

  }

  void SaveBDL(const string &filename)
  {
    ofstream fo(filename,ios_base::out|ios_base::binary);
    ZCHECK(fo.good())<<"Cannot open file "<<filename<<endl;

    fo<<"VMBundleAdjustement {\n\n";
    fo<<"xImageSize "<<imginfos_[0].size[0]<<" yImageSize "<<imginfos_[0].size[1]<<"\n";
    fo<<"ArePointOk 1 NbViewOk "<<cameras_.size()<<" NbPointOk "<<points_.size()<<"\n";

    int nbinlier=0;
    for (zuint i=0; i<points_.size(); i++) nbinlier+=points_[i].pos2ds.size();
    fo<<"NbInlier "<<nbinlier<<" NbOutlier 0 MaxSquareErrorDistance 4 Lambda 1e-06 OldChi2 0 NewChi2 0\n";
    fo<<"\n";

    for (zuint i=0; i<cameras_.size(); i++) {
      fo<<"Camera "<<i<<" {\nNumber "<<i<<" Flag 1 Param 1\n";
      fo<<cameras_[i].camera.P()<<"\n\n}\n\n";
    }

    for (zuint i=0; i<points_.size(); i++) {
      fo<<"Point "<<i<<" { "<<ToHomogeneous(points_[i].pos3d)<<" Flag 1 Param 3\n";
      for (zuint j=0; j<points_[i].pos2ds.size(); j++) {
        fo<<points_[i].pos2ds[j].img_id<<" 1 ";
        if (imginfos_.size()>1)
          fo<<points_[i].pos2ds[j].pos2d + Vector<2,DataType>(imginfos_[points_[i].pos2ds[j].img_id].size)/2<<' ';
        else
          fo<<points_[i].pos2ds[j].pos2d + Vector<2,DataType>(imginfos_[0].size)/2<<' ';
      }
      fo<<" }\n";
    }

    fo<<"}\n";
    fo.close();
  }

  void BDLCameraDecompose(Matrix<3,4,DataType> &P, Matrix<3,3,DataType> &K, Matrix<3,3,DataType> &R, Vector<3,DataType> &T)
  {
    Vector<3,DataType> q1(P.Row(0));
    Vector<3,DataType> q2(P.Row(1));
    Vector<3,DataType> q3(P.Row(2));

    DataType p = 1/(q3.Len());
    R.Row(2)= p * q3;  //r3 = p * q3;
    DataType u0 = p * p * (q1.Dot(q3));
    DataType v0 = p * p * (q2.Dot(q3));
    DataType alpha_u = sqrt(p*p*(q1.Dot(q1)) - u0*u0);
    DataType alpha_v = sqrt(p*p*(q2.Dot(q2)) - v0*v0);
    R.Row(0)= p * (q1 - (u0*q3))/alpha_u;  //r1 = p * (q1 - (u0*q3))/alpha_u;
    R.Row(1)= p * (q2 - (v0*q3))/alpha_v;  //r2 = p * (q2 - (v0*q3))/alpha_v;
    T[2] = p * P(2,3);
    T[0] = p * (P(0,3) - u0*P(2,3)) / alpha_u;
    T[1] = p * (P(1,3) - v0*P(2,3)) / alpha_v;

    K.Identical();
    K(0,0)=alpha_u;
    K(1,1)=alpha_v;
    K(0,2)=u0;
    K(1,2)=v0;

    // Check whether the determinant of the rotation of the extrinsic is positive one.
    if (R.Determinant() < 0)
    {
      Matrix<3,4,DataType> E;
      Dress(E)=(Dress(-R),Dress(-T));
      Matrix<3,4,DataType> P2=K*E;

      // Redo the decomposition.
      q1=Vector<3,DataType>(P2(0,0),P2(0,1),P2(0,2));
      q2=Vector<3,DataType>(P2(1,0),P2(1,1),P2(1,2));
      q3=Vector<3,DataType>(P2(2,0),P2(2,1),P2(2,2));

      p = 1/(q3.Len());
      R.Row(2)= p * q3;  //r3 = p * q3;
      u0 = p * p * FastDot(q1,q3);
      v0 = p * p * FastDot(q2,q3);
      alpha_u = Sqrt(p*p*FastDot(q1,q1) - u0*u0);
      alpha_v = Sqrt(p*p*FastDot(q2,q2) - v0*v0);
      R.Row(0)= p * (q1 - (u0*q3))/alpha_u;  //r1 = p * (q1 - (u0*q3))/alpha_u;
      R.Row(1)= p * (q2 - (v0*q3))/alpha_v;  //r2 = p * (q2 - (v0*q3))/alpha_v;
      T[2] = p * P(2,3);
      T[0] = p * (P(0,3) - u0*P(2,3)) / alpha_u;
      T[1] = p * (P(1,3) - v0*P(2,3)) / alpha_v;
      K.Identical();
      K(0,0)=alpha_u;
      K(1,1)=alpha_v;
      K(0,2)=u0;
      K(1,2)=v0;
    }
  }

  void LoadBDL(const string &filename)
  {
    Clear();
    BraceFile bf;
    ZCHECK(bf.LoadFile(filename));

    // Convert
    BraceNode bundleNode=bf.GetFirstNode("VMBundleAdjustement");
    string tmp;
    int ncam, npoint;
    {
      istringstream iss(bundleNode.GetFirstNodeInclude("NbViewOk").GetText());
      while(!iss.fail()) {
        iss>>tmp;
        if (tmp=="NbViewOk") iss>>ncam;
        if (tmp=="NbPointOk") iss>>npoint;
      }
    }

    cameras_.reserve(ncam);
    for (BraceNode cameraNode=bundleNode.GetFirstNodeInclude("Camera");cameraNode.IsValid();cameraNode.GotoNextSiblingInclude("Camera"))
    {
      string data;
      cameraNode.GetChildrenText(data);
      istringstream iss(data);
      cameras_.push_back(SceneData<DataType>::CameraInfo());
      iss>>tmp>>tmp>>tmp>>tmp>>tmp>>tmp;
      Matrix<3,4,DataType> P;
      iss>>P;
      Matrix<3,3,DataType> K,R;
      Vector<3,DataType> T;
      BDLCameraDecompose(P,K,R,T);
      imginfos_.push_back(SceneData<DataType>::ImageInfo());
      imginfos_.back().size=Vector2i(K(0,2)*2,K(1,2)*2);
      imginfos_.back().focal=(K(0,0)+K(1,1))/2.0;
//       K(0,2)=0;
//       K(1,2)=0;
      cameras_.back().camera.SetK(K);
      cameras_.back().camera.SetRT(R,T);
      cameras_.back().camera.MakeP();
      cameras_.back().status=CAMERA_GOOD;
    }

    points_.reserve(npoint);
    for (BraceNode pointNode=bundleNode.GetFirstNodeInclude("Point");pointNode.IsValid();pointNode.GotoNextSiblingInclude("Point"))
    {
      string data;
      pointNode.GetChildrenText(data);
      istringstream iss(data);
      points_.push_back(SceneData<DataType>::Point());
      string tmp;
      Vector<4,DataType> X;
      int id,status;
      Vector<2,DataType> x;
      iss>>X>>tmp>>status>>tmp>>tmp;
      points_.back().pos3d=FromHomogeneous(X);
      points_.back().status=status==1?POINT_GOOD:POINT_BADP;

      while(true)
      {
        iss>>id>>status>>x;
        if (iss.fail()) break;
        points_.back().pos2ds.push_back(SceneData<DataType>::Point2D());
        points_.back().pos2ds.back().status = status==1?POINT_GOOD:POINT_BADP;
        points_.back().pos2ds.back().img_id = id;
        points_.back().pos2ds.back().pos2d = x-Vector<2,DataType>(imginfos_[id].size/2);
      }
    }
  }

  void TakeOutPair(PairData<DataType> &data, int x0, int x1)
  {
    data.cameras[0]=cameras_[x0].camera;
    data.cameras[1]=cameras_[x1].camera;
    for (zuint i=0; i<points_.size(); i++)
    {
      Point2D *p0=NULL,*p1=NULL;
      for (zuint j=0; j<points_[i].pos2ds.size(); j++)
      {
        if (points_[i].pos2ds[j].img_id==x0) p0=&(points_[i].pos2ds[j]);
        else if (points_[i].pos2ds[j].img_id==x1) p1=&(points_[i].pos2ds[j]);
      }
      if (p0!=NULL && p1!=NULL)
      {
        data.oriidx.push_back(i);
        data.pos2ds[0].push_back(p0->pos2d);
        data.pos2ds[1].push_back(p1->pos2d);
        data.ptidx[0].push_back(p0->pt_idx);
        data.ptidx[1].push_back(p1->pt_idx);
        data.pos3ds.push_back(points_[i].pos3d);
        data.ferror.push_back(points_[i].ferror);
        data.perror.push_back(points_[i].perror);
        data.status.push_back(points_[i].status);
      }
    }
  }
  void PutBackPair(const PairData<DataType> &data, int x0, int x1)
  {
    cameras_[x0].camera=data.cameras[0];
    cameras_[x1].camera=data.cameras[1];
    for (zuint i=0; i<data.pos3ds.size(); i++)
    {
      Point2D *p0=NULL,*p1=NULL;
      int idx=data.oriidx[i];
      for (zuint j=0; j<points_[idx].pos2ds.size(); j++)
      {
        if (points_[i].pos2ds[j].img_id==x0) p0=&(points_[i].pos2ds[j]);
        else if (points_[i].pos2ds[j].img_id==x1) p1=&(points_[i].pos2ds[j]);
      }
      if (p0!=NULL && p1!=NULL)
      {
        points_[idx].pos3d=data.pos3ds[i];
        points_[idx].ferror=data.ferror[i];
        points_[idx].perror=data.perror[i];
        points_[idx].status=data.status[i];
      }
    }
  }
};

const zuint32 SceneData<zfloat32>::Point2D::BAD = MAX_UINT32;
const zuint32 SceneData<zfloat32>::SIFTMatch::BAD = MAX_UINT32;

typedef SceneData<zfloat32> SceneDataf64;
}

